/*
 * CBasicDemo.cpp
 *
 *  Created on: Sep 23, 2011
 *      Author: welcome
 */

#include "CBasicDemo.h"
#include <GLES2/gl2.h>

namespace GGEngine {

CBasicDemo::CBasicDemo() {
	m_cameraPosition.setValue(btScalar(0.f), btScalar(0.f), btScalar(0.f));
	m_cameraUp.setValue(btScalar(0.f), btScalar(1.f), btScalar(0.f));
	m_cameraTargetPosition.setValue(btScalar(0.f), btScalar(0.f), btScalar(0.f));
	m_dynamicsWorld = 0;
	m_cameraDistance = 15.0;
	m_debugMode = 0;
	m_ele = 20.f;
	m_azi = 0.f;
	m_forwardAxis = 2;
	m_screenWidth = 0;
	m_screenHeight = 0;
	m_frustumZNear = 1.f;
	m_frustumZFar = 10000.f;
	m_ortho = 0;
	m_enableshadows = false;
	m_defaultContactProcessingThreshold = BT_LARGE_FLOAT;
	m_ShootBoxInitialSpeed = 40.f;
	m_shootBoxShape = 0;
	m_texturehandle = 0;
	m_textureenabled = false;
	m_textureinitialized = false;
}

CBasicDemo::~CBasicDemo() {
	if (m_shootBoxShape)
		delete m_shootBoxShape;
	int i;
	for (i = 0; i < m_shapecaches.size(); i++) {
		m_shapecaches[i]->~ShapeCache();
		btAlignedFree(m_shapecaches[i]);
	}
	m_shapecaches.clear();
	exitPhysics();
}
void CBasicDemo::exitPhysics() {

	int i;
	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) {
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState()) {
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject(obj);
		delete obj;
	}

	//delete collision shapes
	for (int j = 0; j < m_collisionShapes.size(); j++) {
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	m_collisionShapes.clear();

	delete m_dynamicsWorld;

	delete m_solver;

	delete m_broadphase;

	delete m_dispatcher;

	delete m_collisionConfiguration;
}
void CBasicDemo::initGraphic() {

	GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2),
			btScalar(1.0) };
	GLfloat light_diffuse[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0),
			btScalar(1.0) };
	GLfloat light_specular[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0),
			btScalar(1.0) };
	/*	light_position is NOT default value	*/

	GLfloat light_position0[] = { btScalar(1.0), btScalar(10.0), btScalar(1.0),
			btScalar(0.0) };
	GLfloat light_position1[] = { btScalar(-1.0), btScalar(-10.0), btScalar(
			-1.0), btScalar(0.0) };

	glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
	glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
	glLightfv(GL_LIGHT0, GL_POSITION, light_position0);

	glLightfv(GL_LIGHT1, GL_AMBIENT, light_ambient);
	glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse);
	glLightfv(GL_LIGHT1, GL_SPECULAR, light_specular);
	glLightfv(GL_LIGHT1, GL_POSITION, light_position1);

	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_LIGHT1);

	glShadeModel(GL_SMOOTH);
	glEnable(GL_DEPTH_TEST);
	glDepthFunc(GL_LESS);

	glClearColor(btScalar(0.7), btScalar(0.7), btScalar(0.7), btScalar(0));

}

void CBasicDemo::gluLookAt(GLfloat eyex, GLfloat eyey, GLfloat eyez,
		GLfloat centerx, GLfloat centery, GLfloat centerz, GLfloat upx,
		GLfloat upy, GLfloat upz) {
	GLfloat m[16];
	GLfloat x[3], y[3], z[3];
	GLfloat mag;

	/* Make rotation matrix */

	/* Z vector */
	z[0] = eyex - centerx;
	z[1] = eyey - centery;
	z[2] = eyez - centerz;
	mag = sqrt(z[0] * z[0] + z[1] * z[1] + z[2] * z[2]);
	if (mag) { /* mpichler, 19950515 */
		z[0] /= mag;
		z[1] /= mag;
		z[2] /= mag;
	}

	/* Y vector */
	y[0] = upx;
	y[1] = upy;
	y[2] = upz;

	/* X vector = Y cross Z */
	x[0] = y[1] * z[2] - y[2] * z[1];
	x[1] = -y[0] * z[2] + y[2] * z[0];
	x[2] = y[0] * z[1] - y[1] * z[0];

	/* Recompute Y = Z cross X */
	y[0] = z[1] * x[2] - z[2] * x[1];
	y[1] = -z[0] * x[2] + z[2] * x[0];
	y[2] = z[0] * x[1] - z[1] * x[0];

	/* mpichler, 19950515 */
	/* cross product gives area of parallelogram, which is < 1.0 for
	 * non-perpendicular unit-length vectors; so normalize x, y here
	 */

	mag = sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
	if (mag) {
		x[0] /= mag;
		x[1] /= mag;
		x[2] /= mag;
	}

	mag = sqrt(y[0] * y[0] + y[1] * y[1] + y[2] * y[2]);
	if (mag) {
		y[0] /= mag;
		y[1] /= mag;
		y[2] /= mag;
	}

#define M(row,col)  m[col*4+row]
	M(0, 0) = x[0];
	M(0, 1) = x[1];
	M(0, 2) = x[2];
	M(0, 3) = 0.0;
	M(1, 0) = y[0];
	M(1, 1) = y[1];
	M(1, 2) = y[2];
	M(1, 3) = 0.0;
	M(2, 0) = z[0];
	M(2, 1) = z[1];
	M(2, 2) = z[2];
	M(2, 3) = 0.0;
	M(3, 0) = 0.0;
	M(3, 1) = 0.0;
	M(3, 2) = 0.0;
	M(3, 3) = 1.0;
#undef M
	glMultMatrixf(m);

	/* Translate Eye to Origin */
	glTranslatef(-eyex, -eyey, -eyez);

}
void CBasicDemo::updateCamera() {

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	btScalar rele = m_ele * btScalar(0.01745329251994329547);// rads per deg
	btScalar razi = m_azi * btScalar(0.01745329251994329547);// rads per deg

	btQuaternion rot(m_cameraUp, razi);

	btVector3 eyePos(0, 0, 0);

	eyePos[m_forwardAxis] = -m_cameraDistance;

	btVector3 forward(eyePos[0], eyePos[1], eyePos[2]);
	if (forward.length2() < SIMD_EPSILON) {
		forward.setValue(1.f, 0.f, 0.f);
	}

	btVector3 right = m_cameraUp.cross(forward);
	btQuaternion roll(right, -rele);

	eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos;

	m_cameraPosition[0] = eyePos.getX();
	m_cameraPosition[1] = eyePos.getY();
	m_cameraPosition[2] = eyePos.getZ();
	m_cameraPosition += m_cameraTargetPosition;

	if (m_screenWidth == 0 && m_screenHeight == 0)
		return;

	btScalar aspect;
	btVector3 extents;

	aspect = m_screenWidth / (btScalar) m_screenHeight;
	extents.setValue(aspect * 1.0f, 1.0f, 0);

	if (m_ortho) {
		// reset matrix
		glLoadIdentity();

		extents *= m_cameraDistance;
		btVector3 lower = m_cameraTargetPosition - extents;
		btVector3 upper = m_cameraTargetPosition + extents;
		//gluOrtho2D(lower.x, upper.x, lower.y, upper.y);
		glOrthof(lower.getX(), upper.getX(), lower.getY(), upper.getY(), -1000,
				1000);

		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
	} else {
		glFrustumf((-aspect * m_frustumZNear), (aspect * m_frustumZNear),
				(-m_frustumZNear), (m_frustumZNear), (m_frustumZNear),
				(m_frustumZFar));
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		gluLookAt(m_cameraPosition[0], m_cameraPosition[1],
				m_cameraPosition[2], m_cameraTargetPosition[0],
				m_cameraTargetPosition[1], m_cameraTargetPosition[2],
				m_cameraUp.getX(), m_cameraUp.getY(), m_cameraUp.getZ());
	}

}
#define btglMultMatrix glMultMatrixf
void CBasicDemo::drawOpenGL(btScalar* m, const btCollisionShape* shape,
		const btVector3& color, int debugMode, const btVector3& worldBoundsMin,
		const btVector3& worldBoundsMax) {

	if (shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE) {

		btVector3 org(m[12], m[13], m[14]);
		btVector3 dx(m[0], m[1], m[2]);
		btVector3 dy(m[4], m[5], m[6]);
		const btBoxShape* boxShape = static_cast<const btBoxShape*> (shape);
		btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
		dx *= halfExtent[0];
		dy *= halfExtent[1];
		glColor4f(1, 1, 1, 1);
		glDisable(GL_LIGHTING);
		glLineWidth(2);

		GLfloat vertices[12] = { 0.0 };

		glEnableClientState(GL_VERTEX_ARRAY);

		btVector3 tmp;
		tmp = org - dx - dy;
		vertices[0] = tmp[0];
		vertices[1] = tmp[1];
		vertices[2] = tmp[2];
		tmp = org - dx + dy;
		vertices[3] = tmp[0];
		vertices[4] = tmp[1];
		vertices[5] = tmp[2];
		tmp = org + dx + dy;
		vertices[6] = tmp[0];
		vertices[7] = tmp[1];
		vertices[8] = tmp[2];
		tmp = org + dx - dy;
		vertices[9] = tmp[0];
		vertices[10] = tmp[1];
		vertices[11] = tmp[2];

		glVertexPointer(3, GL_FLOAT, 0, tmp);

		glDrawArrays(GL_LINE_LOOP, 0, 4);

		glDisableClientState(GL_VERTEX_ARRAY);

		return;
	} else if ((shape->getShapeType() == BOX_SHAPE_PROXYTYPE) && (debugMode
			& btIDebugDraw::DBG_FastWireframe)) {
		btVector3 org(m[12], m[13], m[14]);
		btVector3 dx(m[0], m[1], m[2]);
		btVector3 dy(m[4], m[5], m[6]);
		btVector3 dz(m[8], m[9], m[10]);
		const btBoxShape* boxShape = static_cast<const btBoxShape*> (shape);
		btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
		dx *= halfExtent[0];
		dy *= halfExtent[1];
		dz *= halfExtent[2];

		GLfloat vertices[24] = { 0.0 };

		glEnableClientState(GL_VERTEX_ARRAY);

		btVector3 tmp;
		tmp = org - dx - dy - dz;
		vertices[0] = tmp[0];
		vertices[1] = tmp[1];
		vertices[2] = tmp[2];

		tmp = org + dx - dy - dz;
		vertices[3] = tmp[0];
		vertices[4] = tmp[1];
		vertices[5] = tmp[2];

		tmp = org + dx + dy - dz;
		vertices[6] = tmp[0];
		vertices[7] = tmp[1];
		vertices[8] = tmp[2];

		tmp = org - dx + dy - dz;
		vertices[9] = tmp[0];
		vertices[10] = tmp[1];
		vertices[11] = tmp[2];

		tmp = org - dx + dy + dz;
		vertices[12] = tmp[0];
		vertices[13] = tmp[1];
		vertices[14] = tmp[2];

		tmp = org + dx + dy + dz;
		vertices[15] = tmp[0];
		vertices[16] = tmp[1];
		vertices[17] = tmp[2];

		tmp = org + dx - dy + dz;
		vertices[18] = tmp[0];
		vertices[19] = tmp[1];
		vertices[20] = tmp[2];

		tmp = org - dx - dy + dz;
		vertices[21] = tmp[0];
		vertices[22] = tmp[1];
		vertices[23] = tmp[2];

		glVertexPointer(3, GL_FLOAT, 0, vertices);
		glDrawArrays(GL_LINE_LOOP, 0, 8);
		glDisableClientState(GL_VERTEX_ARRAY);

		glEnableClientState(GL_VERTEX_ARRAY);

		tmp = org + dx - dy - dz;
		vertices[0] = tmp[0];
		vertices[1] = tmp[1];
		vertices[2] = tmp[2];

		tmp = org + dx - dy + dz;
		vertices[3] = tmp[0];
		vertices[4] = tmp[1];
		vertices[5] = tmp[2];

		tmp = org + dx + dy - dz;
		vertices[6] = tmp[0];
		vertices[7] = tmp[1];
		vertices[8] = tmp[2];

		tmp = org + dx + dy + dz;
		vertices[9] = tmp[0];
		vertices[10] = tmp[1];
		vertices[11] = tmp[2];

		tmp = org - dx - dy - dz;
		vertices[12] = tmp[0];
		vertices[13] = tmp[1];
		vertices[14] = tmp[2];

		tmp = org - dx + dy - dz;
		vertices[15] = tmp[0];
		vertices[16] = tmp[1];
		vertices[17] = tmp[2];

		tmp = org - dx - dy + dz;
		vertices[18] = tmp[0];
		vertices[19] = tmp[1];
		vertices[20] = tmp[2];

		tmp = org - dx + dy + dz;
		vertices[21] = tmp[0];
		vertices[22] = tmp[1];
		vertices[23] = tmp[2];
		glVertexPointer(3, GL_FLOAT, 0, vertices);
		glDrawArrays(GL_LINES, 0, 8);
		glDisableClientState(GL_VERTEX_ARRAY);
		return;
	}
	glPushMatrix();
	btglMultMatrix(m);
	if (shape->getShapeType() == UNIFORM_SCALING_SHAPE_PROXYTYPE) {
		const btUniformScalingShape* scalingShape =
				static_cast<const btUniformScalingShape*> (shape);
		const btConvexShape* convexShape = scalingShape->getChildShape();
		float scalingFactor = (float) scalingShape->getUniformScalingFactor();
		{
			btScalar tmpScaling[4][4] = { { scalingFactor, 0, 0, 0 }, { 0,
					scalingFactor, 0, 0 }, { 0, 0, scalingFactor, 0 }, { 0, 0,
					0, 1 } };

			drawOpenGL((btScalar*) tmpScaling, convexShape, color, debugMode,
					worldBoundsMin, worldBoundsMax);
		}
		glPopMatrix();
		return;
	}
	if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) {
		const btCompoundShape* compoundShape =
				static_cast<const btCompoundShape*> (shape);
		for (int i = compoundShape->getNumChildShapes() - 1; i >= 0; i--) {
			btTransform childTrans = compoundShape->getChildTransform(i);
			const btCollisionShape* colShape = compoundShape->getChildShape(i);
			btScalar childMat[16];
			childTrans.getOpenGLMatrix(childMat);
			drawOpenGL(childMat, colShape, color, debugMode, worldBoundsMin,
					worldBoundsMax);
		}

	} else {
		if (m_textureenabled && (!m_textureinitialized)) {
			GLubyte* image = new GLubyte[256 * 256 * 3];
			for (int y = 0; y < 256; ++y) {
				const int t = y >> 4;
				GLubyte* pi = image + y * 256 * 3;
				for (int x = 0; x < 256; ++x) {
					const int s = x >> 4;
					const GLubyte b = 180;
					GLubyte c = b + ((s + (t & 1)) & 1) * (255 - b);
					pi[0] = pi[1] = pi[2] = c;
					pi += 3;
				}
			}

			glGenTextures(1, (GLuint*) &m_texturehandle);
			glBindTexture(GL_TEXTURE_2D, m_texturehandle);
			glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE);
			glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
					GL_LINEAR_MIPMAP_LINEAR);
			glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER,
					GL_LINEAR_MIPMAP_LINEAR);
			glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
			glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
			//			gluBuild2DMipmaps(GL_TEXTURE_2D, 3, 256, 256, GL_RGB,
			//					GL_UNSIGNED_BYTE, image);
			delete[] image;
		}

		glMatrixMode(GL_TEXTURE);
		glLoadIdentity();
		glScalef(0.025f, 0.025f, 0.025f);
		glMatrixMode(GL_MODELVIEW);

		static const GLfloat planex[] = { 1, 0, 0, 0 };
		static const GLfloat planez[] = { 0, 0, 1, 0 };
		//		glTexGenvf(GL_S, GL_OBJECT_PLANE, planex);
		//		glTexGenvf(GL_T, GL_OBJECT_PLANE, planez);
		//		//		glTexGeni(GL_S, GL_TEXTURE_GEN_MODE, GL_OBJECT_LINEAR);
		//		//		glTexGeni(GL_T, GL_TEXTURE_GEN_MODE, GL_OBJECT_LINEAR);
		//		glEnable( GL_TEXTURE_GEN_S);
		//		glEnable( GL_TEXTURE_GEN_T);
		//		glEnable( GL_TEXTURE_GEN_R);
		m_textureinitialized = true;

		//drawCoordSystem();

		//glPushMatrix();
		glEnable(GL_COLOR_MATERIAL);
		if (m_textureenabled) {
			glEnable(GL_TEXTURE_2D);
			glBindTexture(GL_TEXTURE_2D, m_texturehandle);
		} else {
			glDisable(GL_TEXTURE_2D);
		}

		glColor4f(color.x(), color.y(), color.z(), 1.0f);

		bool useWireframeFallback = true;

		if (!(debugMode & btIDebugDraw::DBG_DrawWireframe)) {
			///you can comment out any of the specific cases, and use the default

			///the benefit of 'default' is that it approximates the actual collision shape including collision margin
			//int shapetype=m_textureenabled?MAX_BROADPHASE_COLLISION_TYPES:shape->getShapeType();
			int shapetype = shape->getShapeType();
			switch (shapetype) {

			case SPHERE_SHAPE_PROXYTYPE: {
				const btSphereShape* sphereShape =
						static_cast<const btSphereShape*> (shape);
				float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
				//drawSphere(radius, 10, 10);
				useWireframeFallback = false;
				break;
			}

			case BOX_SHAPE_PROXYTYPE: {
				const btBoxShape* boxShape =
						static_cast<const btBoxShape*> (shape);
				btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();

				static int indices[36] = { 0, 1, 2, 3, 2, 1, 4, 0, 6, 6, 0, 2,
						5, 1, 4, 4, 1, 0, 7, 3, 1, 7, 1, 5, 5, 4, 7, 7, 4, 6,
						7, 2, 3, 7, 6, 2 };

				btVector3
						vertices[8] = { btVector3(halfExtent[0], halfExtent[1],
								halfExtent[2]), btVector3(-halfExtent[0],
								halfExtent[1], halfExtent[2]), btVector3(
								halfExtent[0], -halfExtent[1], halfExtent[2]),
								btVector3(-halfExtent[0], -halfExtent[1],
										halfExtent[2]), btVector3(
										halfExtent[0], halfExtent[1],
										-halfExtent[2]), btVector3(
										-halfExtent[0], halfExtent[1],
										-halfExtent[2]), btVector3(
										halfExtent[0], -halfExtent[1],
										-halfExtent[2]), btVector3(
										-halfExtent[0], -halfExtent[1],
										-halfExtent[2]) };
#if 1
				int si = 36;
				for (int i = 0; i < si; i += 3) {
					const btVector3& v1 = vertices[indices[i]];
					;
					const btVector3& v2 = vertices[indices[i + 1]];
					const btVector3& v3 = vertices[indices[i + 2]];
					btVector3 normal = (v3 - v1).cross(v2 - v1);
					normal.normalize();
					glNormal3f(normal.getX(), normal.getY(), normal.getZ());

					GLfloat vertices[9] = { 0.0 };

					glEnableClientState(GL_VERTEX_ARRAY);

					vertices[0] = v1.x();
					vertices[1] = v1.y();
					vertices[2] = v1.z();

					vertices[3] = v2.x();
					vertices[4] = v2.y();
					vertices[5] = v2.z();

					vertices[6] = v3.x();
					vertices[7] = v3.y();
					vertices[8] = v3.z();

					glVertexPointer(3, GL_FLOAT, 0, vertices);
					glDrawArrays(GL_LINES, 0, 3);
					glDisableClientState(GL_VERTEX_ARRAY);
				}
#endif

				useWireframeFallback = false;
				break;
			}
			case STATIC_PLANE_PROXYTYPE: {
				const btStaticPlaneShape* staticPlaneShape =
						static_cast<const btStaticPlaneShape*> (shape);
				btScalar planeConst = staticPlaneShape->getPlaneConstant();
				const btVector3& planeNormal =
						staticPlaneShape->getPlaneNormal();
				btVector3 planeOrigin = planeNormal * planeConst;
				btVector3 vec0, vec1;
				btPlaneSpace1(planeNormal, vec0, vec1);
				btScalar vecLen = 100.f;
				btVector3 pt0 = planeOrigin + vec0 * vecLen;
				btVector3 pt1 = planeOrigin - vec0 * vecLen;
				btVector3 pt2 = planeOrigin + vec1 * vecLen;
				btVector3 pt3 = planeOrigin - vec1 * vecLen;

				GLfloat vertices[12] = { 0.0 };

				glEnableClientState(GL_VERTEX_ARRAY);

				vertices[0] = pt0.getX();
				vertices[1] = pt0.getY();
				vertices[2] = pt0.getZ();

				vertices[3] = pt1.getX();
				vertices[4] = pt1.getY();
				vertices[5] = pt1.getZ();

				vertices[6] = pt2.getX();
				vertices[7] = pt2.getY();
				vertices[8] = pt2.getZ();

				vertices[9] = pt3.getX();
				vertices[10] = pt3.getY();
				vertices[11] = pt3.getZ();

				glVertexPointer(3, GL_FLOAT, 0, vertices);
				glDrawArrays(GL_LINES, 0, 4);
				glDisableClientState(GL_VERTEX_ARRAY);

				break;

			}
			case MULTI_SPHERE_SHAPE_PROXYTYPE: {
				const btMultiSphereShape* multiSphereShape =
						static_cast<const btMultiSphereShape*> (shape);

				btTransform childTransform;
				childTransform.setIdentity();

				for (int i = multiSphereShape->getSphereCount() - 1; i >= 0; i--) {
					btSphereShape sc(multiSphereShape->getSphereRadius(i));
					childTransform.setOrigin(
							multiSphereShape->getSpherePosition(i));
					btScalar childMat[16];
					childTransform.getOpenGLMatrix(childMat);
					drawOpenGL(childMat, &sc, color, debugMode, worldBoundsMin,
							worldBoundsMax);
				}

				break;
			}
			default: {
				if (shape->isConvex()) {
					const btConvexPolyhedron
							* poly =
									shape->isPolyhedral() ? ((btPolyhedralConvexShape*) shape)->getConvexPolyhedron()
											: 0;
					if (poly) {
						int i = 0;
						glEnableClientState(GL_VERTEX_ARRAY);
						for (i = 0; i < poly->m_faces.size(); i++) {
							btVector3 centroid(0, 0, 0);
							int numVerts = poly->m_faces[i].m_indices.size();
							if (numVerts > 2) {
								btVector3
										v1 =
												poly->m_vertices[poly->m_faces[i].m_indices[0]];
								for (int v = 0; v
										< poly->m_faces[i].m_indices.size() - 2; v++) {

									btVector3
											v2 =
													poly->m_vertices[poly->m_faces[i].m_indices[v
															+ 1]];
									btVector3
											v3 =
													poly->m_vertices[poly->m_faces[i].m_indices[v
															+ 2]];
									btVector3 normal = (v3 - v1).cross(v2 - v1);
									normal.normalize();
									glNormal3f(normal.getX(), normal.getY(),
											normal.getZ());

									GLfloat vertices[9] = { 0.0 };

									vertices[0] = v1.x();
									vertices[1] = v1.y();
									vertices[2] = v1.z();

									vertices[0] = v2.x();
									vertices[1] = v2.y();
									vertices[2] = v2.z();

									vertices[0] = v3.x();
									vertices[1] = v3.y();
									vertices[2] = v3.z();

									glVertexPointer(3, GL_FLOAT, 0, vertices);
									glDrawArrays(GL_TRIANGLES, 0, 3);
								}
							}
						}
						glDisableClientState(GL_VERTEX_ARRAY);
					} else {
						ShapeCache* sc = cache((btConvexShape*) shape);
						//glutSolidCube(1.0);
						btShapeHull* hull = &sc->m_shapehull/*(btShapeHull*)shape->getUserPointer()*/;

						if (hull->numTriangles() > 0) {
							int index = 0;
							const unsigned int* idx = hull->getIndexPointer();
							const btVector3* vtx = hull->getVertexPointer();

							glEnableClientState(GL_VERTEX_ARRAY);

							for (int i = 0; i < hull->numTriangles(); i++) {
								int i1 = index++;
								int i2 = index++;
								int i3 = index++;
								btAssert(i1 < hull->numIndices () &&
										i2 < hull->numIndices () &&
										i3 < hull->numIndices ());

								int index1 = idx[i1];
								int index2 = idx[i2];
								int index3 = idx[i3];
								btAssert(index1 < hull->numVertices () &&
										index2 < hull->numVertices () &&
										index3 < hull->numVertices ());

								btVector3 v1 = vtx[index1];
								btVector3 v2 = vtx[index2];
								btVector3 v3 = vtx[index3];
								btVector3 normal = (v3 - v1).cross(v2 - v1);
								normal.normalize();
								glNormal3f(normal.getX(), normal.getY(),
										normal.getZ());
								GLfloat vertices[9] = { 0.0 };

								vertices[0] = v1.x();
								vertices[1] = v1.y();
								vertices[2] = v1.z();

								vertices[0] = v2.x();
								vertices[1] = v2.y();
								vertices[2] = v2.z();

								vertices[0] = v3.x();
								vertices[1] = v3.y();
								vertices[2] = v3.z();
								glVertexPointer(3, GL_FLOAT, 0, vertices);
								glDrawArrays(GL_TRIANGLES, 0, 3);
							}
							glDisableClientState(GL_VERTEX_ARRAY);
						}
					}
				}
			}
			}

		}

		glNormal3f(0, 1, 0);
		/// for polyhedral shapes
		if (debugMode == btIDebugDraw::DBG_DrawFeaturesText
				&& (shape->isPolyhedral())) {
			btPolyhedralConvexShape* polyshape =
					(btPolyhedralConvexShape*) shape;

			{
				glColor4f(1.f, 1.f, 1.f, 1.f);
				int i;
				for (i = 0; i < polyshape->getNumVertices(); i++) {
					btVector3 vtx;
					polyshape->getVertex(i, vtx);
					char buf[12];
					sprintf(buf, " %d", i);
					//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
				}

				for (i = 0; i < polyshape->getNumPlanes(); i++) {
					btVector3 normal;
					btVector3 vtx;
					polyshape->getPlane(normal, vtx, i);
					//btScalar d = vtx.dot(normal);

					//char buf[12];
					//sprintf(buf," plane %d",i);
					//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);

				}
			}

		}

#ifdef USE_DISPLAY_LISTS

		if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
		{
			GLuint dlist = OGL_get_displaylist_for_shape((btCollisionShape * )shape);
			if (dlist)
			{
				glCallList(dlist);
			}
			else
			{
#else
		if (shape->isConcave() && !shape->isInfinite()) {
			btConcaveShape* concaveMesh = (btConcaveShape*) shape;

			GlDrawcallback drawCallback;
			drawCallback.m_wireframe = (debugMode
					& btIDebugDraw::DBG_DrawWireframe) != 0;

			concaveMesh->processAllTriangles(&drawCallback, worldBoundsMin,
					worldBoundsMax);

		}
#endif

#ifdef USE_DISPLAY_LISTS
	}
}
#endif

	}
	glPopMatrix();
}

void CBasicDemo::renderScene(int pass) {
	btScalar m[16];
	btMatrix3x3 rot;
	rot.setIdentity();
	const int numObjects = m_dynamicsWorld->getNumCollisionObjects();
	btVector3 wireColor(1, 0, 0);
	for (int i = 0; i < numObjects; i++) {
		btCollisionObject* colObj =
				m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(colObj);
		if (body && body->getMotionState()) {
			btDefaultMotionState* myMotionState =
					(btDefaultMotionState*) body->getMotionState();
			myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
			rot = myMotionState->m_graphicsWorldTrans.getBasis();
		} else {
			colObj->getWorldTransform().getOpenGLMatrix(m);
			rot = colObj->getWorldTransform().getBasis();
		}
		btVector3 wireColor(1.f, 1.0f, 0.5f); //wants deactivation
		if (i & 1)
			wireColor = btVector3(0.f, 0.0f, 1.f);
		///color differently for active, sleeping, wantsdeactivation states
		if (colObj->getActivationState() == 1) //active
		{
			if (i & 1) {
				wireColor += btVector3(1.f, 0.f, 0.f);
			} else {
				wireColor += btVector3(.5f, 0.f, 0.f);
			}
		}
		if (colObj->getActivationState() == 2) //ISLAND_SLEEPING
		{
			if (i & 1) {
				wireColor += btVector3(0.f, 1.f, 0.f);
			} else {
				wireColor += btVector3(0.f, 0.5f, 0.f);
			}
		}

		btVector3 aabbMin, aabbMax;
		m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin, aabbMax);

		aabbMin -= btVector3(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
		aabbMax += btVector3(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);

		if (!(getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) {
			switch (pass) {
			case 0:
				drawOpenGL(m, colObj->getCollisionShape(), wireColor,
						getDebugMode(), aabbMin, aabbMax);
				break;
			case 1:
				//				drawShadow(m, m_sundirection * rot,
				//						colObj->getCollisionShape(), aabbMin, aabbMax);
				break;
			case 2:
				//				drawOpenGL(m, colObj->getCollisionShape(), wireColor
				//						* btScalar(0.3), 0, aabbMin, aabbMax);
				break;
			}
		}
	}
}

btRigidBody* CBasicDemo::localCreateRigidBody(float mass,
		const btTransform& startTransform, btCollisionShape* shape) {
	btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
	bool isDynamic = (mass != 0.f);
	btVector3 localInertia(0, 0, 0);
	if (isDynamic)
		shape->calculateLocalInertia(mass, localInertia);
#define USE_MOTIONSTATE 1
#ifdef USE_MOTIONSTATE
	btDefaultMotionState* myMotionState = new btDefaultMotionState(
			startTransform);

	btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, shape,
			localInertia);

	btRigidBody* body = new btRigidBody(cInfo);
	body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);

#else
	btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);
	body->setWorldTransform(startTransform);
#endif//
	m_dynamicsWorld->addRigidBody(body);

	return body;
}
CBasicDemo::ShapeCache* CBasicDemo::cache(btConvexShape* shape) {
	ShapeCache* sc = (ShapeCache*) shape->getUserPointer();
	if (!sc) {
		sc = new (btAlignedAlloc(sizeof(ShapeCache),16)) ShapeCache(shape);
		sc->m_shapehull.buildHull(shape->getMargin());
		m_shapecaches.push_back(sc);
		shape->setUserPointer(sc);
		/* Build edges	*/
		const int ni = sc->m_shapehull.numIndices();
		const int nv = sc->m_shapehull.numVertices();
		const unsigned int* pi = sc->m_shapehull.getIndexPointer();
		const btVector3* pv = sc->m_shapehull.getVertexPointer();
		btAlignedObjectArray<ShapeCache::Edge*> edges;
		sc->m_edges.reserve(ni);
		edges.resize(nv * nv, 0);
		for (int i = 0; i < ni; i += 3) {
			const unsigned int* ti = pi + i;
			const btVector3 nrm = btCross(pv[ti[1]] - pv[ti[0]], pv[ti[2]]
					- pv[ti[0]]).normalized();
			for (int j = 2, k = 0; k < 3; j = k++) {
				const unsigned int a = ti[j];
				const unsigned int b = ti[k];
				ShapeCache::Edge*& e = edges[btMin(a, b) * nv + btMax(a, b)];
				if (!e) {
					sc->m_edges.push_back(ShapeCache::Edge());
					e = &sc->m_edges[sc->m_edges.size() - 1];
					e->n[0] = nrm;
					e->n[1] = -nrm;
					e->v[0] = a;
					e->v[1] = b;
				} else {
					e->n[1] = nrm;
				}
			}
		}
	}
	return (sc);
}

btVector3 CBasicDemo::getRayTo(int x, int y) {
	if (m_ortho) {

		btScalar aspect;
		btVector3 extents;
		aspect = m_screenWidth / (btScalar) m_screenHeight;
		extents.setValue(aspect * 1.0f, 1.0f, 0);

		extents *= m_cameraDistance;
		btVector3 lower = m_cameraTargetPosition - extents;
		btVector3 upper = m_cameraTargetPosition + extents;

		btScalar u = x / btScalar(m_screenWidth);
		btScalar v = (m_screenHeight - y) / btScalar(m_screenHeight);

		btVector3 p(0, 0, 0);
		p.setValue((1.0f - u) * lower.getX() + u * upper.getX(), (1.0f - v)
				* lower.getY() + v * upper.getY(),
				m_cameraTargetPosition.getZ());
		return p;
	}

	float top = 1.f;
	float bottom = -1.f;
	float nearPlane = 1.f;
	float tanFov = (top - bottom) * 0.5f / nearPlane;
	float fov = btScalar(2.0) * btAtan(tanFov);

	btVector3 rayFrom = getCameraPosition();
	btVector3 rayForward = (getCameraTargetPosition() - getCameraPosition());
	rayForward.normalize();
	float farPlane = 10000.f;
	rayForward *= farPlane;

	btVector3 rightOffset;
	btVector3 vertical = m_cameraUp;

	btVector3 hor;
	hor = rayForward.cross(vertical);
	hor.normalize();
	vertical = hor.cross(rayForward);
	vertical.normalize();

	float tanfov = tanf(0.5f * fov);

	hor *= 2.f * farPlane * tanfov;
	vertical *= 2.f * farPlane * tanfov;

	btScalar aspect;

	aspect = m_screenWidth / (btScalar) m_screenHeight;

	hor *= aspect;

	btVector3 rayToCenter = rayFrom + rayForward;
	btVector3 dHor = hor * 1.f / float(m_screenWidth);
	btVector3 dVert = vertical * 1.f / float(m_screenHeight);

	btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
	rayTo += btScalar(x) * dHor;
	rayTo -= btScalar(y) * dVert;
	return rayTo;
}
void CBasicDemo::setShootBoxShape() {
	if (!m_shootBoxShape) {
		btBoxShape* box = new btBoxShape(btVector3(.5f, .5f, .5f));
		box->initializePolyhedralFeatures();
		m_shootBoxShape = box;
	}
}
void CBasicDemo::shootBox(const btVector3& destination) {

	if (m_dynamicsWorld) {
		float mass = 1.f;
		btTransform startTransform;
		startTransform.setIdentity();
		btVector3 camPos = getCameraPosition();
		startTransform.setOrigin(camPos);

		setShootBoxShape();

		btRigidBody* body = this->localCreateRigidBody(mass, startTransform,
				m_shootBoxShape);

		body->setLinearFactor(btVector3(1, 1, 1));
		//body->setRestitution(1);

		btVector3 linVel(destination[0] - camPos[0],
				destination[1] - camPos[1], destination[2] - camPos[2]);
		linVel.normalize();
		linVel *= m_ShootBoxInitialSpeed;

		body->getWorldTransform().setOrigin(camPos);
		body->getWorldTransform().setRotation(btQuaternion(0, 0, 0, 1));
		body->setLinearVelocity(linVel);
		body->setAngularVelocity(btVector3(0, 0, 0));
		body->setCcdMotionThreshold(0.5);
		body->setCcdSweptSphereRadius(0.9f);
	}
}

void CBasicDemo::reshape(int w, int h) {
	m_screenWidth = w;
	m_screenHeight = h;

	glViewport(0, 0, w, h);
	updateCamera();
}
void CBasicDemo::animate() {

	initGraphic();

	updateCamera();

	if (m_dynamicsWorld) {
		if (m_enableshadows) {

			glClear(GL_STENCIL_BUFFER_BIT);
			glEnable(GL_CULL_FACE);

			renderScene(0);

			glDisable(GL_LIGHTING);
			glDepthMask(GL_FALSE);
			glDepthFunc(GL_LEQUAL);
			glEnable(GL_STENCIL_TEST);
			glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE);
			glStencilFunc(GL_ALWAYS, 1, 0xFFFFFFFFL);
			glFrontFace(GL_CCW);
			glStencilOp(GL_KEEP, GL_KEEP, GL_INCR);
			renderScene(1);
			glFrontFace(GL_CW);
			glStencilOp(GL_KEEP, GL_KEEP, GL_DECR);
			renderScene(1);
			glFrontFace(GL_CCW);

			//			glPolygonMode(GL_FRONT, GL_FILL);
			//			glPolygonMode(GL_BACK, GL_FILL);

			glShadeModel(GL_SMOOTH);
			glEnable(GL_DEPTH_TEST);
			glDepthFunc(GL_LESS);
			glEnable(GL_LIGHTING);
			glDepthMask(GL_TRUE);
			glCullFace(GL_BACK);
			glFrontFace(GL_CCW);
			glEnable(GL_CULL_FACE);
			glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);

			glDepthFunc(GL_LEQUAL);
			glStencilFunc(GL_NOTEQUAL, 0, 0xFFFFFFFFL);
			glStencilOp(GL_KEEP, GL_KEEP, GL_KEEP);
			glDisable(GL_LIGHTING);
			renderScene(2);
			glEnable(GL_LIGHTING);
			glDepthFunc(GL_LESS);
			glDisable(GL_STENCIL_TEST);
			glDisable(GL_CULL_FACE);
		} else {
			glDisable(GL_CULL_FACE);
			renderScene(0);
		}
		glDisable(GL_LIGHTING);
		glColor4f(0, 0, 0, 1.0f);
		glDisable(GL_LIGHTING);
	}
	updateCamera();
}
void CBasicDemo::initPhysics() {

	setShadows(true);
	setCameraDistance(btScalar(SCALING * 50.));

	//End

	///collision configuration contains default setup for memory, collision setup
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	//m_collisionConfiguration->setConvexConvexMultipointIterations();

	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);

	m_broadphase = new btDbvtBroadphase();

	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* sol =
			new btSequentialImpulseConstraintSolver;
	m_solver = sol;

	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase,
			m_solver, m_collisionConfiguration);

	m_dynamicsWorld->setGravity(btVector3(0, -10, 0));

	///create a few basic rigid bodies
	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),
			btScalar(50.), btScalar(50.)));
	//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);

	m_collisionShapes.push_back(groundShape);

	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0, -50, 0));

	//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
	{
		btScalar mass(0.);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0, 0, 0);
		if (isDynamic)
			groundShape->calculateLocalInertia(mass, localInertia);

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(
				groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState,
				groundShape, localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		//add the body to the dynamics world
		m_dynamicsWorld->addRigidBody(body);
	}

	{
		//create a few dynamic rigidbodies
		// Re-using the same collision is better for memory usage and performance

		btCollisionShape* colShape = new btBoxShape(btVector3(SCALING * 1,
				SCALING * 1, SCALING * 1));
		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
		m_collisionShapes.push_back(colShape);

		/// Create Dynamic Objects
		btTransform startTransform;
		startTransform.setIdentity();

		btScalar mass(1.f);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0, 0, 0);
		if (isDynamic)
			colShape->calculateLocalInertia(mass, localInertia);

		float start_x = START_POS_X - ARRAY_SIZE_X / 2;
		float start_y = START_POS_Y;
		float start_z = START_POS_Z - ARRAY_SIZE_Z / 2;

		for (int k = 0; k < ARRAY_SIZE_Y; k++) {
			for (int i = 0; i < ARRAY_SIZE_X; i++) {
				for (int j = 0; j < ARRAY_SIZE_Z; j++) {
					startTransform.setOrigin(SCALING * btVector3(btScalar(2.0
							* i + start_x), btScalar(20 + 2.0 * k + start_y),
							btScalar(2.0 * j + start_z)));

					//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
					btDefaultMotionState* myMotionState =
							new btDefaultMotionState(startTransform);
					btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,
							myMotionState, colShape, localInertia);
					btRigidBody* body = new btRigidBody(rbInfo);

					m_dynamicsWorld->addRigidBody(body);
				}
			}
		}
	}

}
}
